#include "BPEPhysicsWorld.h"

BPEPhysicsWorld::BPEPhysicsWorld(){
	cWorld = shared_ptr<BPECollisionWorld>();
};

BPEPhysicsWorld::BPEPhysicsWorld(const BPEPoint3d &worldSize){
	cWorld = shared_ptr<BPECollisionWorld>(new BPECollisionWorld(BPEPoint3d(0,0,0),worldSize));
};
static int counter = 0;

void BPEPhysicsWorld::addPhysicsObject(shared_ptr<BPEPhysicsObject> &pObj){
	
	vector3df *edges = new vector3df[8];
	pObj->getNode()->updateAbsolutePosition();
	aabbox3d<f32> box = pObj->getNode()->getTransformedBoundingBox();
	box.getEdges(edges);
	shared_array<BPEPoint3d> boxEdges(new BPEPoint3d[8]);


	for(int i = 0; i < 8; i++)
		boxEdges[i] = BPEPoint3d(edges[i].X,edges[i].Y,edges[i].Z);




	
	BPEPoint3d center(pObj->getNode()->getTransformedBoundingBox().getCenter().X,pObj->getNode()->getTransformedBoundingBox().getCenter().Y,pObj->getNode()->getTransformedBoundingBox().getCenter().Z);


	BPEBox3d boundingBox(center,boxEdges);
	delete edges;

	
	shared_ptr<BPEBoundingShape> bObject = shared_ptr<BPEBoundingBox>(new BPEBoundingBox(boundingBox));
	cWorld->addBoundingShape(bObject);
	

	pObj->setboundingShape(bObject);
	bpePhysicsObjects.push_back(pObj);
//three lines for testing purposes
		counter++;
	//if(counter == 1)
	//	pObj->setRotation(BPEVector3d(0,0,(counter-1)*25));
	this->updateBoundingShape(bpePhysicsObjects.size()-1);
	
};

void BPEPhysicsWorld::setUniversalSpeed(const BPEVector3d &vect){
	universalSpeed = vect;
};
void BPEPhysicsWorld::addUniversalForce(const BPEVector3d &vect){
	universalForces.push_back(vect);
};
void BPEPhysicsWorld::resetUniversalForces(){
	universalForces.clear();
};
void BPEPhysicsWorld::updateAllBoundingShapes(){

	int size = bpePhysicsObjects.size();
	for(int i = 0; i < size; i++)
		updateBoundingShape(i);


};
void BPEPhysicsWorld::updateBoundingShape(const int& objCode){

	int size = bpePhysicsObjects.size();
	if(size > objCode){
		vector3df center = bpePhysicsObjects[objCode]->getNode()->getTransformedBoundingBox().getCenter();
		BPEPoint3d newPoint(center.X,center.Y,center.Z);
		cWorld->updateBoundingBox(objCode,newPoint,bpePhysicsObjects[objCode]->getRotation()*pi/180);

	}
	
};
void BPEPhysicsWorld::physicsRoutine(){
	//physics phase 1 - give new speeds and positions and update the bounding boxes
	vector<bool> onMove;
  vector<BPEVector3d> speeds;

	this->physicsRoutinePhase1(onMove, speeds);
	this->updateAllBoundingShapes();


	vector<BPECollisionData> collisionPairs;
	vector<BPEVector3d> fixSpeeds;

	cWorld->collisionRoutine(onMove, speeds ,collisionPairs, fixSpeeds);

	
	
	this->physicsRoutinePhase2(collisionPairs, fixSpeeds);
	

	
};
void BPEPhysicsWorld::physicsRoutinePhase1(vector<bool> &onMove, vector<BPEVector3d> &speeds){

	

  speeds.assign(bpePhysicsObjects.size(),BPEVector3d(0,0,0));
  for(vector<	shared_ptr<BPEPhysicsObject>  >::size_type i = 0; i <bpePhysicsObjects.size(); i++){
		if(bpePhysicsObjects[i]->getSpeed().getMagnitude() > 0 && bpePhysicsObjects[i]->getPhysicsOn()){
			onMove.push_back(true);
			BPEVector3d newPos = bpePhysicsObjects[i]->getPosition().add(bpePhysicsObjects[i]->getSpeed());
			bpePhysicsObjects[i]->setPosition(newPos);	
			this->updateBoundingShape(i);
		}
		else
			onMove.push_back(false);

    speeds[i] = bpePhysicsObjects[i]->getSpeed();

	}

	
	
};


void BPEPhysicsWorld::physicsRoutinePhase2(const vector<BPECollisionData> &collisionPairs,const vector<BPEVector3d> &fixSpeeds){


	int size  = bpePhysicsObjects.size();
	int collObjSize = collisionPairs.size();

	//first handle collided objects 


	for(int i = 0; i < collObjSize; i++){
    //fix the positions of the collided objects
    bpePhysicsObjects[collisionPairs[i].firstObjectCode]->setPosition(
      bpePhysicsObjects[collisionPairs[i].firstObjectCode]->getPosition().add(
      fixSpeeds[collisionPairs[i].firstObjectCode])
    );
    
    bpePhysicsObjects[collisionPairs[i].secondObjectCode]->setPosition(
      bpePhysicsObjects[collisionPairs[i].secondObjectCode]->getPosition().add(
      fixSpeeds[collisionPairs[i].secondObjectCode])
    );

    //assign new speeds to the collided objects
		fullCollisionSolution(collisionPairs[i].firstObjectCode,collisionPairs[i].secondObjectCode,
				collisionPairs[i].collisionVector);
	}


	//handle all the objects and give new speeds to the not colliding ones
	for(int i = 0; i < size ; i++){
		//if it is not a physics object ignore it completely
		if(!bpePhysicsObjects[i]->getPhysicsOn()){
				continue;
		}
		//reset all objects forces
		bpePhysicsObjects[i]->resetAllForces();

		//apply universal forces to all objects
		int forcesSize = universalForces.size();
		for(int j = 0 ; j < forcesSize; j ++){
			bpePhysicsObjects[i]->addForce(universalForces[j]);
		}

		
		bool isNotCollisionObj = true; 
		//for( int j = 0; j < collObjSize; j++){
		//for every object check if it collided
		//	if(i == collisionPairs[j].firstObjectCode || i == collisionPairs[j].secondObjectCode){
		//		isNotCollisionObj = false;
		//		break;
		//	}
				
		//}

		//if the objects did not collide
		if(isNotCollisionObj){
			//apply universal forces to all object
			int forcesSize = universalForces.size();
			for(int j = 0 ; j < forcesSize; j ++){
				bpePhysicsObjects[i]->addForce(universalForces[j]);
			}
			//calculate new speed and apply gravity	
			bpePhysicsObjects[i]->calculateSpeed();
			bpePhysicsObjects[i]->setSpeed(bpePhysicsObjects[i]->getSpeed().add(universalSpeed));	
		}

	}
	
	
	

};
//private functions
void BPEPhysicsWorld::fullCollisionSolution(const int &colliderCode1, const int &colliderCode2,BPEVector3d collisionNormal){
	
	//1,2 stands for object1 object 2

	BPEPhysicsObject *collider1 = bpePhysicsObjects[colliderCode1].get();
	BPEPhysicsObject *collider2 = bpePhysicsObjects[colliderCode2].get();


	BPEVector3d oldSpeed1 = collider1->getSpeed();
	BPEVector3d newSpeed1 = BPEVector3d();

	BPEVector3d oldSpeed2 = collider2->getSpeed();
	BPEVector3d newSpeed2 = BPEVector3d();
	

	if(collider1->getPhysicsOn()){

		
		newSpeed1 = ((oldSpeed1*(collider1->getMass() - collider2->getMass())).add( oldSpeed2*(2*collider2->getMass())))/(collider1->getMass() + collider2->getMass());
		newSpeed1 = newSpeed1.getReflectionAgaintNormal(collisionNormal);

		collider1->setSpeed(newSpeed1);
	}

	if(collider2->getPhysicsOn()){

		newSpeed2 = ((oldSpeed2*(collider2->getMass() - collider1->getMass())).add( oldSpeed1*(2*collider1->getMass())))/(collider2->getMass() + collider1->getMass());
		newSpeed2 = newSpeed2.getReflectionAgaintNormal(collisionNormal);

		collider2->setSpeed(newSpeed2);
	}
	
	

};
void BPEPhysicsWorld::simpleCollisionSolution(const vector<BPECollisionData> &collisionPairs){




  for(vector<BPECollisionData>::size_type i = 0; i < collisionPairs.size(); i++){

		BPEPhysicsObject *first = bpePhysicsObjects[collisionPairs[i].firstObjectCode].get();
		BPEPhysicsObject *second = bpePhysicsObjects[collisionPairs[i].secondObjectCode].get();



		shared_ptr<BPEBoundingShape> firstBound = cWorld->getBoundingBox(collisionPairs[i].firstObjectCode);
		shared_ptr<BPEBoundingShape> secondBound = cWorld->getBoundingBox(collisionPairs[i].secondObjectCode);

		BPEVector3d centersConnection = BPEVector3d(firstBound->getCenter().getHead(),
										secondBound->getCenter().getHead());


		BPEVector3d temp = collisionPairs[i].collisionVector;
		temp.setMagnitude(collisionPairs[i].overlappingDepth);
/*
		//when depth is fixed use something like this
		if(centersConnection.findRadsDifference(collisionPairs[i].collisionVector) > pi/2){	
			temp.inverse();
		}
	
		if(first->getPhysicsOn() && first->getSpeed().getMagnitude() > 0)
			first->setPosition(first->getPosition().add(temp));
		temp.inverse();
		if(second->getPhysicsOn() && second->getSpeed().getMagnitude() > 0)
			second->setPosition(second->getPosition().add(temp));

*/
		BPEVector3d firstSpeed = first->getSpeed();
		BPEVector3d secondSpeed = second->getSpeed();

		firstSpeed.inverse();
		secondSpeed.inverse();




		firstSpeed.setMagnitude(firstSpeed.getMagnitude() * 0.1);
		secondSpeed.setMagnitude(secondSpeed.getMagnitude() * 0.1);


		//while(first->getNode()->getTransformedBoundingBox().intersectsWithBox(second->getNode()->getTransformedBoundingBox())){
		double x;
		while(firstBound->collide(secondBound.get(),BPEVector3d(),x)){	
			if(first->getPhysicsOn() && first->getSpeed().getMagnitude() > 0){
				BPEVector3d test = first->getPosition();
				first->setPosition(first->getPosition().add(firstSpeed));
				this->updateBoundingShape(collisionPairs[i].firstObjectCode);
			}
			if(second->getPhysicsOn() && second->getSpeed().getMagnitude() > 0){	
				second->setPosition(second->getPosition().add(secondSpeed));
				this->updateBoundingShape(collisionPairs[i].secondObjectCode);
			}
				

				
				


		}
		
	}
	
	
};

